package com.my.learn.exercise.data.struct.graph;


/*
 * 创建人：baimiao
 * 创建时间：2025/5/14 14:30
 *
 */

import java.util.*;

public class AStartFindPath {
    public static void main(String[] args) {

        int[][] graph = {{0, 0, 0, 1, 0}, {0, 1, 0, 0, 0}, {0, 0, 1, 0, 1}, {1, 0, 0, 0, 1}, {0, 1, 0, 0, 0}};
        int[] start = {0, 0};
        int[] end = {4, 4};
        AStartFindPath aStart = new AStartFindPath();
        List<Node> list = aStart.findPath(graph, start, end);
    }


    private List<Node> findPath(int[][] graph, int[] start, int[] end) {
        PriorityQueue<Node> queue = new PriorityQueue<>(Comparator.comparing(Node::getH));
        Node begin = new Node(start[0], start[1]);
//        begin.calculateHeuristic(end);
        begin.g = 0;
        begin.f = begin.g + begin.h;
        queue.add(begin);
        Node destination = new Node(start[0], start[1]);
        List<Node> visited = new ArrayList<>();
        int[][] direction = {{0, 1}, {0, -1}, {1, 0}, {-1, 0}};
        int width = graph[0].length;
        int height = graph.length;
        while (!queue.isEmpty()) {
            Node current = queue.poll();
            if (current.equals(destination)) {
                break;
            }
            for (int[] step : direction) {
                int nx = current.getX() + step[0];
                int ny = current.getY() + step[1];
                if (nx < 0 || ny < 0 || nx >= width || ny >= height) {
                    continue;
                }
                if (graph[nx][ny] == 1) {
                    continue;
                }
                Node next = new Node(nx, ny);
                queue.add(next);
                if (!visited.contains(next)) {
                    next.setPre(current);
                    next.g = current.getG() + 1; //每次走一步
                    next.calculateHeuristic(destination);
                    next.f = next.g + next.h;
                    visited.add(next);
                } else {
                    int ng = current.getG() + 1;
                    if (ng < next.getG()) {
                        current.g = ng;
                    }
                }
            }


        }

        return null;

    }


    private class Node {
        private int x;
        private int y;
        private Node pre;

        //启发函数距离，可以是欧几里得（可以对角线），曼哈顿距离（上下左右）四个位置每次移动一个距离
        public int g; // 起点到当前节点距离
        public int h; //当前到终点距离
        public int f; //f = g+h

        public Node() {
        }

        public Node(int x, int y) {
            this.x = x;
            this.y = y;
        }


        private void calculateHeuristic(Node end) {
            h = Math.abs(end.getX() - x) + Math.abs(end.getY() - y);
        }

        public int getX() {
            return x;
        }

        public void setX(int x) {
            this.x = x;
        }

        public int getY() {
            return y;
        }

        public void setY(int y) {
            this.y = y;
        }

        public Node getPre() {
            return pre;
        }

        public void setPre(Node pre) {
            this.pre = pre;
        }

        public int getG() {
            return g;
        }

        public int getH() {
            return h;
        }

        public int getF() {
            return f;
        }

        @Override
        public boolean equals(Object o) {
            if (o == null || getClass() != o.getClass()) return false;
            Node node = (Node) o;
            return x == node.x && y == node.y;
        }

        @Override
        public int hashCode() {
            return Objects.hash(x, y);
        }
    }


}
